#!/usr/bin/python
#-*- coding: UTF-8 -*-   
#according to yolo's boundingbox ,calculate the human's position and to track it  
import rospy
import sys
#sys.path.append('/home/robocup/catkin_robocup/devel/lib/python2.7/dist-packages')
import math
import PyKDL
import numpy
import numpy.matlib
import time
import copy
from darknet_ros_msgs.msg import BoundingBoxes
from mavros_msgs.msg import State
from std_msgs.msg import Int64MultiArray, String
from collections import deque
from geometry_msgs.msg import Twist, Vector3, PoseStamped, TwistStamped, Point
from catch_actors.msg import coordinate, auction, auction_value
from ros_actor_cmd_pose_plugin_msgs.msg import ActorInfo
import message_filters
from ObstacleAvoid import ObstacleAviod

class Track_and_Annouce:
    def __init__(self,vehicle_type, vehicle_id):
        self.vehicle_type = vehicle_type
        self.vehicle_id = vehicle_id
        self.target_id = []
        self.target_class = []
        self.uav_num = 6
        self.actor_pos = [auction() for i in range(self.uav_num)]
        self.count = [0 for i in range(6)]     # 6 个 actor 
        self.find = False 
        self.theta = -math.pi/4  
        self.human = 1.57
        self.gimble = 0.162   
        self.holdon = False    
        self.human_num = 0 
        self.firsttrack_num = 0 
        self.countcout = 0   
        self.numvalue_sub = [None]*(self.uav_num) 
        self.track_sub = [None]*(self.uav_num)
        self.red_man_sub = [None]*(self.uav_num)
        self.file=open('result_announceall'+str(self.vehicle_id)+'.txt','w') 
        self.start_yolo_flag = False
        self.coordx_bias = 0
        self.coordy_bias = 0
        #para about tracking    
        self.Kp_xy = 0.6   
        self.kp = 0.6   
        self.kpy =1 
        self.f = 20
        self.counta = 0
        self.vmax = 4
        self.anglemax = 3
        self.twist = Twist()         
        self.cmd = String() 
        self.judge_value = ActorInfo()
        self.HUMAN_CLASS = [ "red", "green", "blue",  "white", "brown", "red"]
        self.track_number = 7                      
        self.numvalue = auction_value()
        self.hover_count = 0
        self.fake_man =[-26.902,13.6572]
        # obstacle avoidance:
        self.safe_dis = 10.0  # distance between uavs for avoidance
        self.safe_height = 0.5
        self.Obstacleavoid = ObstacleAviod()  #ji wu 
        self.subtarget_count = 0
        self.subtarget_length = 0
        self.avoid_flag = False  
        self.subtarget_pos = []
        self.localcall_time = 0.0
        self.pixcall_time = 0.0

        self.vx = 0
        self.vy = 0
        self.vz = 0
        self.actor_judge_undone = [] 

        # searching and tracking:
        self.count_erupt_time = 0
        self.time_start = [0 for i in range(6)]
        self.go_and_search_flag = False
        self.run_flag = False     
        self.track_flag = False
        self.actor_undone = [0,1,2,3,4,5]
        self.human_tracking_flag = False   
        self.human_queue_x = deque(maxlen=4)  
        self.human_queue_y = deque(maxlen=4)  
        self.good_flag = False # 判断是否为我正在追的人
        self.uav_current_pose = Point()
        self.human_trac_flag = False #判断是否追人
        self.uav_current_yaw = 0.0
        self.red_man0_id = 2  # 有两个红色的人，所以在追踪红色人的时候需要机间交互
        self.red_man1_id = 5
        self.red_man_pose = Point() 
        self.red_man_find = [Point() for i in range(2)]
        # self.red_man_over_flag = False
        self.red_man_tracking_flag = 0
        self.red_tracker = 7

        self.go_to_position_flag = False  # 到人的坐标上去
        self.queue_size = 20
        self.uav_queue_x = deque(maxlen=self.queue_size) 
        self.uav_queue_y = deque(maxlen=self.queue_size) 
        self.uav_queue_z = deque(maxlen=self.queue_size) 
        self.uav_queue_q1 = deque(maxlen=self.queue_size) 
        self.uav_queue_q2 = deque(maxlen=self.queue_size) 
        self.uav_queue_q3 = deque(maxlen=self.queue_size) 
        self.uav_queue_q4 = deque(maxlen=self.queue_size) 
        self.time_queue = deque(maxlen = self.queue_size)

        self.bais = numpy.array([[0.0, 3.0], [3.0, 3.0], [0.0, 6.0], [3.0, 6.0], [0.0, 9.0], [3.0, 9.0]]) 
        # self.black_box =  [[[53.2, 54.2],[-9.0, -6.72]], \
        # [[79, 92], [-33.6, -28]],[[3, 9], [34, 36]], [[-35.0, -20.0], [17.0, 33.5]], [[4.0, 22.0], [11.0, 25.0]], [[55, 69.0], [15, 30.3]], \
        # [[71.5, 83.5], [9.2, 18.5]], [[88.3, 100.7], [11.9, 16.2]], [[79, 94.7], [22.2, 34.5]], [[53, 70], [-35, -27]], [[-5.4, 7.0], [-19.4, -10.0]], \
        # [[18.5, 39.5], [-22.5, -5.5]], [[-5.0, 5.0], [-34.0, -22.0]], [[-27.8, -23.5], [-27.5, -15]], [[-36, -24], [-35, -31]], \
        #[[-35.5, -31.3], [-25.5, -13]], [[3.0,8.2],[34.3,35.7]]]
        #[[-35.375, -34.825], [-4, -3]], [[-3.275, -2.725], [-4, -3]], [[20.725, 21.275], [-4, -3]], \
        #[[57.725, 58.275], [-4, -3]], [[83.725, 84.275], [-4, -3]], [[-25.475, -24.925], [3, 4]], [[8.725, 9.275], [3, 4]], \
        #[[26.725, 27.275], [3, 4]], [[70.725, 71.275], [3, 4]], [[90.225, 90.775], [3, 4]],
        black_box = [\
        [[-35.25, -18.51], [16.40, 35.38]],\
        [[2.80, 9.50], [33.50, 36.47]],\
        [[3.80, 23.45], [10.37, 25.62]],\
        [[52.54, 70.54], [14.43, 30.38]],\
        [[71.08, 83.62], [8.38, 18.61]],\
        [[78.63, 94.79], [21.42, 34.51]],\
        [[87.61, 100.91], [11.47, 16.40]],\
        [[-36.45, -34.63], [-4.32, -2.79]],\
        [[-26.59, -24.49], [2.69, 4.23]],\
        [[-4.48, -2.41], [-4.19, -2.85]],\
        [[7.57, 9.60], [2.82, 4.13]],\
        [[19.51, 21.58], [-4.19, -2.84]],\
        [[25.54, 27.60], [2.76, 4.21]],\
        [[51.92, 55.27], [-10.24, -4.81]],\
        [[-36.07, -30.59], [-26.47, -12.54]],\
        [[-28.08, -23.38], [-27.77, -14.45]],\
        [[-36.14, -23.60], [-35.45, -30.39]],\
        [[-6.13, 8.60], [-34.60, -10.48]],\
        [[17.74, 39.49], [-22.51, -5.50]],\
        [[51.2, 70.60], [-37.8, -23.50]],\
	    [[57.10, 66.48],[-24.77,-9.63]],\
        [[78.62, 92.60], [-34.24, -27,73]],\
        [[56.51, 58.57], [-4.19, -2.81]],\
        [[69.65, 71.57], [2.76, 4.20]],\
        [[82.51, 84.64], [-4.21, -2.79]],\
        [[88.72, 91.06], [2.78, 4.20]]]
        self.box_num = len(self.black_box)
                                                                                                                                                                               
        #相机内参
        self.fx = 205.46963709898583
        self.fy = 205.46963709898583
        self.cx = 320.5
        self.cy = 180.5
        #求变化矩阵的相关参数
        self.Q1, self.Q2, self.Q3, self.Q4 = 0,0,0,0
        self.px, self.py, self.pz = 0,0,0
        #绕x,y,z旋转的弧度角度
        self.r, self.p, self.y = 0,0,0
        self.w2u_ROTATION, self.u2w_ROTATION = numpy.matlib.zeros((3,3)),numpy.matlib.zeros((3,3))
        self.c2u_ROTATION = numpy.matlib.zeros((3,3))
        self.c2w_ROTATION = numpy.matlib.zeros((3,3))
        self.u2w_TRASLATION = numpy.matlib.zeros((3,1))
        self.c2u_TRASLATION = numpy.matlib.zeros((3,1))
        self.cInU_POSITION = numpy.matlib.zeros((3,1))
        #目标世界坐标初始化
        self.world_posi = numpy.matlib.zeros((3,1))
        #像素坐标初始化
        self.pix_posi = numpy.matlib.zeros((2,6))
        self.pix_posi_piece = numpy.matlib.zeros((2,1))
        #求解最终世界坐标的中间参数
        self.h2c_ROTATION= numpy.matlib.zeros((3,3))
        self.h2c_pos = numpy.matlib.zeros((3,1))
        self.camera_pos = numpy.matlib.zeros((3,1))
        rospy.init_node(self.vehicle_type+'_'+self.vehicle_id+'track_and_annouce')

        #订阅话题
        self.bbox_sub = rospy.Subscriber("/uav_"+str(self.vehicle_id)+"/darknet_ros/bounding_boxes_track", BoundingBoxes, self.pix_posi_callback)
        self.left_actors_sub = rospy.Subscriber("left_actors",String,self.left_actors_callback)
        # self.judger_sub = rospy.Subscriber("judger_react" , String, self.judger_callback)
        self.vel_sub = rospy.Subscriber('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_flu', Twist, self.uav_velocity_callback)
        self.start_yolo_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+'/start_yolo',String,self.start_yolo_callback)
        self.local_pose_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/local_position/pose", PoseStamped, self.local_pose_callback)
        self.uav_state_sub = rospy.Subscriber(self.vehicle_type+'_'+self.vehicle_id+"/mavros/state", State, self.uav_state_callback)
        for i in range(self.uav_num):  #6个无人机
            if not i == self.vehicle_id:
                self.red_man_sub = rospy.Subscriber(self.vehicle_type+'_'+str(i)+'/red_man_position',Point,self.red_man_pose_receive_callback,i)
            self.numvalue_sub = rospy.Subscriber(self.vehicle_type+'_'+str(i)+'/start_tracking',auction_value, self.numvalue_callback, i)
        #发布话题
        self.actor_pos_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+'/auction', auction, queue_size=1)
        # self.judger_pub = rospy.Publisher('judger',ActorInfo,queue_size=5)
        self.vel_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd_vel_flu', Twist, queue_size=2)
        self.cmd_pub = rospy.Publisher('/xtdrone/'+vehicle_type+'_'+vehicle_id+'/cmd', String, queue_size=2)
        self.numvalue_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+'/start_tracking',auction_value,queue_size = 10)
        self.red_man_pub = rospy.Publisher(self.vehicle_type+'_'+self.vehicle_id+'/red_man_position',Point,queue_size = 5)  # 因为有两个红人，所以需要通过机间通信判断是否要追
        self.actor_blue_pub = rospy.Publisher("/actor_blue_info",ActorInfo,queue_size = 5)
        self.actor_green_pub = rospy.Publisher("/actor_green_info",ActorInfo,queue_size = 5)
        self.actor_white_pub = rospy.Publisher("/actor_white_info",ActorInfo,queue_size = 5)
        self.actor_brown_pub = rospy.Publisher("/actor_brown_info",ActorInfo,queue_size = 5)
        self.actor_red1_pub = rospy.Publisher("/actor_red1_info",ActorInfo,queue_size = 5)
        self.actor_red2_pub = rospy.Publisher("/actor_red2_info",ActorInfo,queue_size = 5)
    def local_pose_callback(self, data):
        #if self.holdon == False:
        #　时间戳存储
        self.localcall_time = data.header.stamp.to_sec()+data.header.stamp.to_nsec()/pow(10.0,9)
        self.Q1 = data.pose.orientation.x
        self.Q2 = data.pose.orientation.y
        self.Q3 = data.pose.orientation.z
        self.Q4 = data.pose.orientation.w
        self.px = data.pose.position.x + self.bais[int(self.vehicle_id)][0]
        self.py = data.pose.position.y + self.bais[int(self.vehicle_id)][1]
        self.pz = data.pose.position.z
        self.uav_queue_q1.append(self.Q1)
        self.uav_queue_q2.append(self.Q2)
        self.uav_queue_q3.append(self.Q3)
        self.uav_queue_q4.append(self.Q4)
        self.uav_queue_x.append(self.px)
        self.uav_queue_y.append(self.py)
        self.uav_queue_z.append(self.pz)
        self.time_queue.append(self.localcall_time)
        self.uav_current_pose.x = self.px
        self.uav_current_pose.y = self.py
        self.uav_current_pose.z = self.pz
        rot = PyKDL.Rotation.Quaternion(self.Q1,self.Q2,self.Q3,self.Q4)
        self.uav_current_yaw =rot.GetRPY()[2]
        while self.uav_current_yaw > math.pi:
            self.uav_current_yaw -= 2.0*math.pi
        while self.uav_current_yaw < -math.pi:
            self.uav_current_yaw += 2.0*math.pi

        #do not consider the same two red man in our view cause their id is same
    
    def left_actors_callback(self,msg):   # 根据裁判系统确定当前剩余敌人（红色除外）
        left = msg.data
        left = left.replace('[',' ')
        left = left.replace(']',' ')
        left = left.replace(',',' ')
        str_left = left.split(' ')
        self.actor_judge_undone = []
        for j in range(len(str_left)):
            if j%2 == 1:
                if str_left[j] == '0':
                    ac_num = 1
                    self.actor_judge_undone.append(ac_num)
                elif str_left[j] == '1':
                    ac_num = 2
                    self.actor_judge_undone.append(ac_num)
                elif str_left[j] == '2':
                    ac_num = 4
                    self.actor_judge_undone.append(ac_num)
                elif str_left[j] == '3':
                    ac_num = 3
                    self.actor_judge_undone.append(ac_num)


    def uav_velocity_callback(self,data):

        self.vx = data.linear.x
        self.vy = data.linear.y
        self.vz = data.linear.z

    def uav_state_callback(self,data):  # 判断ｕａｖ是否已经挂掉
        if data.armed == 'False':
            self.count_erupt_time = 0
            self.numvalue.value = 200.00 + self.track_number 
            self.numvalue_pub.publish(self.numvalue)
            print str(self.vehicle_id) + 'i am died '
            self.file.write(str(self.vehicle_id) + 'i am died ')
            self.file.write('\n')
            self.track_flag = False  # 追踪失败
            self.avoid_flag = False
            self.actor_undone.append(self.track_number)                               
            self.count_erupt_time = 0                                         
            self.track_number = 7 
            around_flag = False      

    def start_yolo_callback(self,msg):
        start = msg.data
        if start == 'gogogo':
            self.start_yolo_flag = True

    def pix_posi_callback(self,data):
        # if data.bounding_boxes == []:
        #    print("no paddle result !")
        # else:
        #print("into result")
        if self.holdon == False:
            # self.pixcall_time = data.header.stamp.to_sec()+data.header.stamp.to_nsec()/pow(10.0,9)
            #　记录时间戳
            print("process result")
            self.pixcall_time = data.image_header.stamp.to_sec()+data.image_header.stamp.to_nsec()/pow(10.0,9)
            self.good_flag = False
            id_all = [0,1,2,3,4,5]
            self.target_id = []
            self.target_class = []         
            for target1 in data.bounding_boxes:
                if target1.id in id_all:    #注意查数/追踪的时候要保证跟刚才追的是同一个
                    if (target1.id ==self.red_man0_id) and (self.red_man0_id in self.target_id):
                        self.target_id.append(self.red_man1_id)
                        self.pix_posi[0,self.red_man1_id] = (target1.xmax + target1.xmin)/2 
                        self.pix_posi[1,self.red_man1_id] = (target1.ymax + target1.ymin)/2
                        self.target_class.append(target1.Class)
                    else:        
                        self.pix_posi[0,target1.id] = (target1.xmax + target1.xmin)/2 
                        self.pix_posi[1,target1.id] = (target1.ymax + target1.ymin)/2
                        self.target_id.append(target1.id)
                        self.target_class.append(target1.Class)
                    self.find = True 
                    if self.track_number == target1.id:
                        self.good_flag = True  # 判断有没有我正在追的人 
                        #id class 该不该存  
            self.human_num = len(self.target_id)

    def numvalue_callback(self,msg,i):
        # 通过机间通信防止多个无人机追踪同一个人
        track_value = msg.value
        if not i == int(self.vehicle_id):
            if track_value < 150.00:
                iddd = int(track_value-100.0)
                self.time_start[iddd] = rospy.get_time()
                while(iddd in self.actor_undone):
                    self.actor_undone.remove(iddd)  #  记录尚未被追踪的敌人
            elif track_value > 150.00 and track_value < 250.00:
                iddd = int(track_value-200.0)
                if (iddd in self.actor_judge_undone) or iddd == 0:
                    self.actor_undone.append(iddd)
                else:
                    while(iddd in self.actor_undone):
                        self.actor_undone.remove(iddd)
                print 'self.actor_undone:', self.actor_undone
                self.file.write('self.actor_undone:'+str(self.actor_undone))
                self.file.write('\n')
                print 'self.actor_judge_undone:', self.actor_judge_undone
                self.file.write('self.actor_judge_undone:'+str(self.actor_judge_undone))
                self.file.write('\n')
                if iddd == 0:
                    self.file.write(' on red is catching over')
                    self.file.write('\n')
                    print ' on red is catching over'
                    self.red_man_tracking_flag = 0
                    self.red_tracker = 7
                self.count[iddd] = 0 
             
    #send msg to task_planning,the number each uav find

    # def judger_callback(self, msg):
    #     success_id = msg.data
    #     for i in range (5):
    #         if success_id == self.HUMAN_CLASS[self.track_number]:
    #             self.track_flag = False
    #             self.numvalue.value = 300.00 + self.track_number                                     
    #             self.numvalue_pub.publish(self.numvalue)
    #             self.track_number = 7
    #             self.count_erupt_time = 0
    #             print str(self.vehicle_id)+'   i catch him !!!!!!' 

    def red_man_pose_receive_callback(self,msg,i):
        self.red_man_pos_compare = msg 
        if self.red_tracker == 7:
            self.red_tracker = int(i) # 记录当前谁在追红色的人
            self.red_man_tracking_flag = 1  #  当前有一个红人正在被追
            self.red_man_find[0] = self.red_man_pos_compare # 记录当前正在被追的红色的人的位置
        elif not self.red_tracker == int(i): # 如果不是7 说明已经有红人正在被追了 
            self.red_man_tracking_flag = 2 # 当前有两个红人正在被追
            self.red_man_find[1] = self.red_man_pos_compare# 记录另一个正在被追的红色的人的位置
        else:
            self.red_man_find[0] = self.red_man_pos_compare # 更新红人位置


    def loop(self):
        rate = rospy.Rate(self.f)
        arrive_point = False
        subtarget_count = 0
        tar_point = Point()
        arrive_count = 0
        avoid_x = 0.0
        avoid_y = 0.0
        avoid_z = 5.0
        next_subtarget_flag = False
        # around_flag = False

        while not rospy.is_shutdown():
            self.countcout += 1
            self.cmd = ''
            #self.find = False
            self.human_trac_flag = False
            around_flag = False
            self.twist.angular.z = 0.0
            self.calcutale_distance()
            # for j in range(6):
            #     if self.time_start[j]>0:
            #         timing = rospy.get_time()
            #         if timing - self.time_start[j] > 120.0:
            #             self.actor_undone.append(j)

            if self.track_flag and self.human_trac_flag:  # 如果没在执行壁障且开始视觉伺服， 判断是否在障碍物内
                if not self.avoid_flag: # 如果没在壁障，则判断是否需要壁障
                    avoid_failed_flag = False                
                    for i in range(self.box_num):  
                        if (self.px > (self.black_box[i][0][0]-2)) and (self.px < (self.black_box[i][0][1]+2)):
                            if (self.py > (self.black_box[i][1][0]-2)) and (self.py < (self.black_box[i][1][1]+2)): # 在障碍物范围内
                                self.avoid_flag = True  # 要到敌人那里去哦
                                print str(self.vehicle_id) + 'is avoiding obstacle'
                                self.file.write(str(self.vehicle_id) + 'is avoiding obstacle')
                                self.file.write('\n')
                                tar_point.x = self.actor_pos[self.track_number].actor_x
                                tar_point.y = self.actor_pos[self.track_number].actor_y
                                tar_point.z = 5.0 
                                if (tar_point.x > (self.black_box[i][0][0]-1.2)) and (tar_point.x < (self.black_box[i][0][1]+1.2)):
                                    if (tar_point.y > (self.black_box[i][1][0]-1.2)) and (tar_point.y < (self.black_box[i][1][1]+1.2)):
                                        self.avoid_flag = False   # 人的坐标也在障碍物内 宣告臂章失败！ 急停
                                        print str(self.vehicle_id)+'   avoiding failed'
                                        self.file.write(str(self.vehicle_id)+'   avoiding failed')
                                        self.file.write('\n')
                                        self.cmd = 'HOVER'               
                                        self.twist.linear.x = 0.0
                                        self.twist.linear.y = 0.0
                                        self.twist.linear.z = 0.0
                                        self.twist.angular.z = 0.0
                                        self.vel_pub.publish(self.twist)
                                        self.cmd_pub.publish(self.cmd)
                                        self.hover_count +=1
                                        print 'self.hover_count:',self.hover_count
                                        self.file.write('self.hover_count:'+str(self.hover_count))
                                        self.file.write('\n')
                                        avoid_failed_flag = True
                                        if self.hover_count >=3: # 如果敌人位置长时间在障碍物内，放弃追踪
                                            self.hover_count = 0
                                            self.count_erupt_time = 0
                                            self.numvalue.value = 200.00 + self.track_number 
                                            self.numvalue_pub.publish(self.numvalue)
                                            print str(self.vehicle_id) + '1111111loseloselose ' + str(self.track_number)
                                            self.file.write(str(self.vehicle_id) + '1111111loseloselose ' + str(self.track_number))
                                            self.file.write('\n')
                                            self.track_flag = False #追踪失败   (后期应该判断是否是因为追踪成功导致的人物消失)
                                            self.avoid_flag = False
                                            self.actor_undone.append(self.track_number)                               
                                            self.count_erupt_time = 0                                         
                                            self.track_number = 7 
                                            around_flag = False        
                                                        
                                if self.avoid_flag and (not avoid_failed_flag): # 壁障
                                    print str(self.vehicle_id) + 'self.uav_current_pose:',self.uav_current_pose
                                    print str(self.vehicle_id) + 'target_pose:   ', tar_point
                                    self.file.write(str(self.vehicle_id) + 'self.uav_current_pose:'+str(self.uav_current_pose))
                                    self.file.write('\n')
                                    self.file.write(str(self.vehicle_id) + 'target_pose:   '+str(tar_point))
                                    self.file.write('\n')
                                    self.count_erupt_time = 0
                                    try:  # 若求不出解，则先像外侧移动
                                        subtarget_pos = self.Obstacleavoid.GetPointList(self.uav_current_pose, tar_point, 1) # current pose, target pose, safe distance
                                        subtarget_length = len(subtarget_pos)
                                        middd_pos = [Point() for k in range(subtarget_length)]
                                        middd_pos = copy.deepcopy(subtarget_pos)
                                        avoid_x = copy.deepcopy(middd_pos[0].x)
                                        avoid_y = copy.deepcopy(middd_pos[0].y)
                                        
                                    except:
                                        dis_list = [0,0,0,0]
                                        dis_list[0] = abs(self.uav_current_pose.x - (self.black_box[i][0][0]-2.0))
                                        dis_list[1] = abs(self.uav_current_pose.x - (self.black_box[i][0][1]+2.0))
                                        dis_list[2] = abs(self.uav_current_pose.y - (self.black_box[i][1][0]-2.0))
                                        dis_list[3] = abs(self.uav_current_pose.y - (self.black_box[i][1][1]+2.0))
                                        dis_min = dis_list.index(min(dis_list))
                                        if dis_min ==0:
                                            avoid_x = self.black_box[i][0][0]-1.0
                                            avoid_y = self.uav_current_pose.y
                                        elif dis_min ==1:
                                            avoid_x = self.black_box[i][0][1]+1.0
                                            avoid_y = self.uav_current_pose.y
                                        elif dis_min ==2:
                                            avoid_x = self.uav_current_pose.x
                                            avoid_y= self.black_box[i][1][0]-1.0
                                        else:
                                            avoid_x = self.uav_current_pose.x
                                            avoid_y = self.black_box[i][1][1]+1.0                                       
                                        subtarget_length = 1 
                                    print str(self.vehicle_id) + 'middd_pos_x:',avoid_x
                                    print str(self.vehicle_id) + 'middd_pos_y:',avoid_y
                                    self.file.write(str(self.vehicle_id) + 'middd_pos_x:'+str(avoid_x))
                                    self.file.write('\n')
                                    self.file.write(str(self.vehicle_id) + 'middd_pos_y:'+str(avoid_y))
                                    self.file.write('\n')
                                    break
                    if not avoid_failed_flag:
                        self.hover_count = 0

                    if (not avoid_failed_flag) and (not self.avoid_flag):
                        if self.track_number < 7:
                            self.human_tracking(self.track_number)  # 不在障碍物范围内，且能看到人就执行视觉伺服程序。
                            print str(self.vehicle_id)+'catching '+str(self.track_number)
                        else:   # 程序错乱 宣告失败
                            self.count_erupt_time = 0
                            print str(self.vehicle_id) + 'luanleluanle ' + str(self.track_number)
                            self.file.write(str(self.vehicle_id) + 'luanleluanle '+str(self.track_number))
                            self.file.write('\n')
                            self.numvalue.value = 300.00
                            self.numvalue_pub.publish(self.numvalue)                                
                            self.track_flag = False #追踪失败   (后期应该判断是否是因为追踪成功导致的人物消失)
                            self.avoid_flag = False
                            self.actor_undone.append(self.track_number)                               
                            self.count_erupt_time = 0                                         
                            self.track_number = 7 
                            around_flag = False  
                            self.cmd = 'HOVER'
                            self.twist.linear.x = 0.0
                            self.twist.linear.y = 0.0
                            self.twist.linear.z = 0.0
                            self.twist.angular.z = 0.0
                            self.vel_pub.publish(self.twist)
                            self.cmd_pub.publish(self.cmd)
                        around_flag = False

            # judge if arrive the target point
            dis = (self.uav_current_pose.x - avoid_x)**2+(self.uav_current_pose.y - avoid_y)**2
            if dis < 2.0:
                arrive_count += 1
                if arrive_count > 2:
                    arrive_point = True
                    arrive_count = 0
                else:
                    arrive_point = False
            else:
                arrive_count = 0
                arrive_point = False

            # actor 从视野消失了怎么办： 等待，转圈，失败
            if self.track_flag and (not self.human_trac_flag) and (not self.avoid_flag): 
                self.count_erupt_time = self.count_erupt_time + 1 
                if (self.count_erupt_time < 1*self.f):
                    pass
                elif (self.count_erupt_time < 3*self.f):
                    self.cmd = 'HOVER'
                    self.twist.linear.x = 0.5*self.twist.linear.x
                    self.twist.linear.y = 0.5*self.twist.linear.y
                    self.twist.linear.z = 0.0
                    self.twist.angular.z = 0.0   
                    self.vel_pub.publish(self.twist)
                    self.cmd_pub.publish(self.cmd)
                    if self.count_erupt_time == 3:
                        print str(self.vehicle_id) + '1111111disapear ' + str(self.track_number)
                        self.file.write(str(self.vehicle_id) + '1111111disapear '+str(self.track_number))
                        self.file.write('\n')
                elif (self.count_erupt_time < 8*self.f):  #转圈圈
                    around_flag = True
                    self.twist.angular.z = -1.0
                    self.twist.linear.x = 0.0
                    self.twist.linear.y = 0.0
                    self.twist.linear.z = 0.0  
                elif self.count_erupt_time >= 8*self.f: #追踪失败
                    self.count_erupt_time = 0
                    self.cmd = 'HOVER'
                    self.twist.linear.x = 0.0
                    self.twist.linear.y = 0.0
                    self.twist.linear.z = 0.0
                    self.twist.angular.z = 0.0   
                    self.vel_pub.publish(self.twist)
                    self.cmd_pub.publish(self.cmd)
                    self.numvalue.value = 200.00 + self.track_number 
                    self.numvalue_pub.publish(self.numvalue)
                    print str(self.vehicle_id) + '1111111loseloselose ' + str(self.track_number)
                    self.file.write(str(self.vehicle_id) + '1111111loseloselose  '+str(self.track_number))
                    self.file.write('\n')
                    self.track_flag = False
                    self.avoid_flag = False
                    self.actor_undone.append(self.track_number)
                    #self.actor_undone.append(self.track_number)
                    self.count_erupt_time = 0
                    #print 'undone_list:     ', self.actor_undone  
                    self.track_number = 7 
                    around_flag = False                
            else:
                self.count_erupt_time = 0   

            if self.avoid_flag and self.track_flag:
                if arrive_point:
                    subtarget_count += 1
                    arrive_point = False
                    if subtarget_count == subtarget_length:
                        self.avoid_flag = False  
                        print str(self.vehicle_id) +'avoid successfully' 
                        self.file.write(str(self.vehicle_id) + 'avoid successfully'+str(self.track_number))
                        self.file.write('\n')                     
                        subtarget_count = 0
                        # next_subtarget_flag = False
                        # if self.go_and_search_flag:
                        #     around_flag = True   # 转圈找人
                    else:
                        avoid_x = middd_pos[subtarget_count].x
                        avoid_y = middd_pos[subtarget_count].y
                        print str(self.vehicle_id) + 'is avoiding obstacle or searching'
                        self.file.write(str(self.vehicle_id) + 'is avoiding obstacle or searching')
                        self.file.write('\n')
                self.get_control_vel(avoid_x, avoid_y, 5.0)
                

            if self.track_flag: 
                if not around_flag:
                    self.twist.angular.z = 0.0
                self.vel_pub.publish(self.twist)
                self.cmd_pub.publish(self.cmd)

            rate.sleep()
        self.file.close() 

    def calcutale_distance(self):
        if self.find:
            self.find = False
            self.holdon = True
            k=0
            self.countcout += 1
            length_time = len(self.uav_queue_q1)
            mid_time = [0.0 for j in range (length_time)]
            self.go_and_search_flag = False
            self.actor_pos = [auction() for i in range(6)]
            # 通过时间戳对比找到最接近的uav位置信息
            for j in range(length_time):
                mid_time[j] = abs(self.time_queue[j]-self.pixcall_time)
            index_mix_time = mid_time.index(min(mid_time))
            # print str(self.vehicle_id)+'index_mix_time:', index_mix_time

            #四元数转欧拉角
            # print self.uav_queue_x[index_mix_time], self.uav_queue_y[index_mix_time], self.uav_queue_z[index_mix_time]
            # 计算定位
            rot = PyKDL.Rotation.Quaternion(self.uav_queue_q1[index_mix_time],self.uav_queue_q2[index_mix_time],self.uav_queue_q3[index_mix_time],self.uav_queue_q4[index_mix_time])
            self.r =rot.GetRPY()[0]
            self.p =rot.GetRPY()[1]
            self.y =rot.GetRPY()[2]
            while self.y > math.pi:
                self.y -= 2.0*math.pi
            while self.y < -math.pi:
                self.y += 2.0*math.pi
            #世界(东北天)到飞机（东北天）ZYX
            x, y, z = self.r,self.p, self.y
            ca1, sa1 = math.cos(z),math.sin(z)
            cb1, sb1 = math.cos(y),math.sin(y)
            cc1, sc1 =  math.cos(x),math.sin(x)
            self.w2u_ROTATION =numpy.matrix([[1,0,0],[0,cc1,sc1],[0,-sc1,cc1]])*numpy.matrix([[cb1,0,-sb1],[0,1,0],[sb1,0,cb1]])* numpy.matrix([[ca1,sa1,0],[-sa1,ca1,0],[0,0,1]])

            self.u2w_ROTATION = (self.w2u_ROTATION).I
            self.u2w_TRASLATION = [[self.uav_queue_x[index_mix_time]], [self.uav_queue_y[index_mix_time]], [self.uav_queue_z[index_mix_time]]]
            
            #相机（右手）到飞机（右手坐标系）（绕xzy）
            x1,z1,y1 = -self.theta, math.pi/2, -math.pi/2
            ca2, sa2 = math.cos(z1),math.sin(z1)
            cb2, sb2 = math.cos(y1),math.sin(y1)
            cc2, sc2 =  math.cos(x1),math.sin(x1)
            self.c2u_ROTATION = numpy.matrix([[cb2,0,-sb2],[0,1,0],[sb2,0,cb2]]) * numpy.matrix([[ca2,sa2,0],[-sa2,ca2,0],[0,0,1]]) * numpy.matrix([[1,0,0],[0,cc2,sc2],[0,-sc2,cc2]])
            self.c2u_TRASLATION = [[0],[0],[-self.gimble]]
            self.cInU_POSITION = self.u2w_ROTATION * [[0],[0],[-self.gimble]]
            
            #飞机（东北天，只有偏航）到世界（东北天）ZYX
            x, y, z = 0, 0, self.y
            ca1, sa1 = math.cos(z),math.sin(z)
            cb1, sb1 = math.cos(y),math.sin(y)
            cc1, sc1 =  math.cos(x),math.sin(x)
            self.w2u_ROTATION =numpy.matrix([[1,0,0],[0,cc1,sc1],[0,-sc1,cc1]])*numpy.matrix([[cb1,0,-sb1],[0,1,0],[sb1,0,cb1]])* numpy.matrix([[ca1,sa1,0],[-sa1,ca1,0],[0,0,1]])
            self.u2w_ROTATION = (self.w2u_ROTATION).I

            self.c2w_ROTATION = self.u2w_ROTATION * self.c2u_ROTATION
            zw = 0.8

            #print 'self.target_id', self.target_id
            for i in self.target_id:
                self.pix_posi_piece = self.pix_posi[...,i]
                R = self.c2w_ROTATION
                self.px = self.uav_queue_x[index_mix_time]
                self.py = self.uav_queue_y[index_mix_time]
                self.pz = self.uav_queue_z[index_mix_time]
                xc = (zw - self.pz)*(self.pix_posi_piece[0]-self.cx)/(R[2,0]*(self.pix_posi_piece[0]-self.cx) + self.fx/self.fy*R[2,1]*(self.pix_posi_piece[1]-self.cy)+R[2,2]*self.fx)
                yc = (zw - self.pz)*(self.pix_posi_piece[1]-self.cy)/(self.fy/self.fx*R[2,0]*(self.pix_posi_piece[0]-self.cx)+R[2,1]*(self.pix_posi_piece[1]-self.cy)+R[2,2]*self.fy)
                zc = (zw - self.pz)/(1/self.fx*R[2,0]*(self.pix_posi_piece[0]-self.cx)+R[2,1]/self.fy*(self.pix_posi_piece[1]-self.cy)+R[2,2])

                xw = xc*R[0,0] + yc*R[0,1] + zc*R[0,2] + self.px
                yw = xc*R[1,0] + yc*R[1,1] + zc*R[1,2] + self.py
                zw = xc*R[2,0] + yc*R[2,1] + zc*R[2,2] + self.pz

                distance_uav_calcactor = math.sqrt(pow((self.px - xw),2)+pow((self.py - yw),2))
                if self.countcout % 100 == 0:
                    print 'distance_uav_calcactor:    ', distance_uav_calcactor
                    print str(i)+'the position is :    ', xw, yw, zw
                announce = False
                # 距离小于30才认为计算出来的位置是正确的
                if distance_uav_calcactor < 30.0 :
                    self.actor_pos[i].actor_x = xw
                    self.actor_pos[i].actor_y = yw
                if distance_uav_calcactor <15.0: # 距离足够近之后才发布给裁判（误差较小）
                    announce = True
                if (i == self.red_man0_id) and (self.red_man_tracking_flag == 1): # 如果是红人，判断是否正在被追
                    if math.sqrt((xw -self.red_man_find[0].x)**2 + (yw -self.red_man_find[0].y)**2) >= 10.0:
                        self.actor_undone.append(self.red_man0_id)
                    while(i in self.actor_undone):                        
                        self.actor_undone.remove(i) 

                if not self.track_flag:
                    if (i == self.red_man0_id) and (self.red_man_tracking_flag == 2):
                        while(i in self.actor_undone):                        
                            self.actor_undone.remove(i) 
                        self.track_number = 7

                if (distance_uav_calcactor < 30.0) and (not self.track_flag) and (self.track_number > 6) and (i in self.actor_undone):  # 没有追踪任务, 赋予追踪任务   （后期再考虑是否需要count【i】==2再追）
                    self.track_number = i
                    self.track_flag = True
                    # print 'distance_uav_calcactor:    ', distance_uav_calcactor
                    # print xw, yw, zw
                    print str(self.vehicle_id) + 'find a man and its track number is ' + str(i) 
                    self.file.write(str(self.vehicle_id) + 'find a man and its track number is ' + str(i))
                    self.file.write('\n')                       
                    self.numvalue.value = 100.00 + self.track_number                                     
                    self.numvalue_pub.publish(self.numvalue)
                    while(self.track_number in self.actor_undone):                        
                        self.actor_undone.remove(self.track_number)
                    self.twist.linear.x = 0.0
                    self.twist.linear.y = 0.0
                    self.twist.linear.z = 0.0
                    self.twist.angular.z = 0.0
                    self.vel_pub.publish(self.twist)
                    self.human_queue_x = deque(maxlen = 4)
                    self.human_queue_y = deque(maxlen = 4)
                    # print 'self.target_id', self.target_id

                if (distance_uav_calcactor < 30.0) and self.track_flag and (i in self.actor_undone) and self.count[int(i)] == 0:   # 已有追踪任务，发布其他actor位置信息呼叫支援
                    self.actor_pos[i].actor_x = xw
                    self.actor_pos[i].actor_y = yw
                    self.actor_pos[i].auction_class = 'actor'
                    self.actor_pos[i].actor_id = float(i)
                    self.actor_pos_pub.publish(self.actor_pos[i])
                    print 'call others to catch ' + str(i) + '!!!!!!!!!'
                    self.file.write(str(self.vehicle_id) + 'call others to catch ' + str(i) + '!!!!!!!!!')
                    self.file.write('\n')    
                    self.count[int(i)] = 1

                if i == self.track_number:
                    self.good_flag = True
                    self.file.write("go tracking")
                    if announce:
                        self.human_queue_x.append(self.actor_pos[i].actor_x)
                        self.human_queue_y.append(self.actor_pos[i].actor_y)
                        if len(self.human_queue_x) == 4:
                            if i == 2:  # 判断是不是假人
                                average_x = float(sum(self.human_queue_x)/4)
                                average_y = float(sum(self.human_queue_y)/4)
                                delta_hhh = math.sqrt((average_x - self.fake_man[0])**2 +(average_y - self.fake_man[1])**2)
                                # print 'delta_hhh,fakefakefake!!!!1', delta_hhh
                                self.file.write(str(self.vehicle_id) + 'delta_hhh,fakefakefake!!!!1')
                                self.file.write('\n') 
                                if delta_hhh < 2:
                                    self.numvalue.value = 200.00 + self.track_number 
                                    self.numvalue_pub.publish(self.numvalue)
                                    print str(self.vehicle_id) + '1111111loseloselose ' + str(self.track_number)
                                    self.file.write(str(self.vehicle_id) + '1111111loseloselose ' + str(self.track_number))
                                    self.file.write('\n') 
                                    self.track_flag = False #追踪失败   (后期应该判断是否是因为追踪成功导致的人物消失)
                                    self.avoid_flag = False
                                    self.actor_undone.append(self.track_number)
                                    #self.actor_undone.append(self.track_number)
                                    self.count_erupt_time = 0
                                    #print 'undone_list:     ', self.actor_undone  
                                    self.track_number = 7 
                                    around_flag = False        
                            if self.track_flag:
                                #self.judge_value = str(self.actor_pos[i].actor_x) + ' '+ str(self.actor_pos[i].actor_y)
                                # mid_position = Point()
                                # leng_mid = len(self.human_queue_x)-1
                                # for j in range (len(self.human_queue_x)):
                                #     mid_position.x += self.human_queue_x[j]
                                #     mid_position.y += self.human_queue_y[j]
                                # mid_position.x = mid_position.x/leng_mid
                                # mid_position.y = mid_position.y/leng_mid
                                # if ((mid_position.x[leng_mid]-mid_position.x)**2+(mid_position.y[leng_mid]-mid_position.y)**2) < 4.0:
                                # 给裁判系统发消息
                                self.judge_value.cls = self.HUMAN_CLASS[int(i)]
                                self.judge_value.x = float(self.actor_pos[i].actor_x + self.coordx_bias)
                                self.judge_value.y = float(self.actor_pos[i].actor_y + self.coordy_bias)
                                #print self.judge_value
                                if (i == 0) and (not self.red_man_tracking_flag == 1):
                                    self.actor_red1_pub.publish(self.judge_value) 
                                elif i ==1:
                                    self.actor_green_pub.publish(self.judge_value)
                                elif i ==2:
                                    self.actor_blue_pub.publish(self.judge_value)
                                elif i ==3:
                                    self.actor_white_pub.publish(self.judge_value)
                                elif i ==4:
                                    self.actor_brown_pub.publish(self.judge_value)
                                elif (i == 0) and (self.red_man_tracking_flag == 1):
                                    self.actor_red2_pub.publish(self.judge_value)   
                            if i == self.red_man0_id :  # 机间交互
                                self.red_man_pose.x = self.actor_pos[i].actor_x
                                self.red_man_pose.y = self.actor_pos[i].actor_y
                                self.red_man_pose.z = int(self.vehicle_id)
                                self.red_man_pub.publish(self.red_man_pose)
            self.holdon = False
            self.target_id=[]
            self.target_class=[]
            if self.good_flag:
                self.human_trac_flag = True
                self.count_erupt_time = 0
        # else:  # if could not see the actor, predict its' position 
        #     if self.track_flag and (self.track_number<6):
        #         length = len(self.human_queue_x)
        #         if length > 1: 
        #             if self.human_queue_x[length-1] - self.human_queue_x[length-2] > 10:
        #                 pre_actor_pose_x = self.human_queue_x[0]
        #                 pre_actor_pose_y = self.human_queue_y[0]
        #                 self.human_queue_x = []
        #                 self.human_queue_y = []
        #             else:
        #                 pre_actor_pose_x = 2*self.human_queue_x[length-1] - self.human_queue_x[length-2]
        #                 pre_actor_pose_y = 2*self.human_queue_y[length-1] - self.human_queue_y[length-2]
        #             self.human_queue_x.append(pre_actor_pose_x)
        #             self.human_queue_y.append(pre_actor_pose_y)
        #             self.judge_value.x = pre_actor_pose_x + self.coordx_bias
        #             self.judge_value.y = pre_actor_pose_y + self.coordy_bias
        #             self.judge_value.cls = self.HUMAN_CLASS[int(self.track_number)]
        #             if (self.track_number == 2) and (self.red_man_tracking_flag == 0):
        #                 self.actor_red1_pub.publish(self.judge_value) 
        #             elif self.track_number == 1:
        #                 self.actor_green_pub.publish(self.judge_value)
        #             elif self.track_number == 0:
        #                 self.actor_blue_pub.publish(self.judge_value)
        #             elif self.track_number == 3:
        #                 self.actor_white_pub.publish(self.judge_value)
        #             elif self.track_number == 4:
        #                 self.actor_brown_pub.publish(self.judge_value)
        #             elif (self.track_number == 2) and (self.red_man_tracking_flag == 1):
        #                 self.actor_red2_pub.publish(self.judge_value)

    def human_tracking(self,track_id):  # 视觉伺服
        self.human_tracking_flag = True
        self.run_flag = False
        z = self.pz/math.cos(self.theta)
        u =  self.pix_posi[0,track_id]
        v = self.pix_posi[1,track_id] 
        u_ =  u - self.cx
        v_ = v- self.cy
        u_velocity = -self.Kp_xy*u_
        v_velocity = -self.Kp_xy*v_
        x_velocity = v_velocity*z/(-v_*math.sin(self.theta)+self.fy*math.cos(self.theta))
        y_velocity = (u_*math.sin(self.theta)*x_velocity+z*u_velocity)/self.fx
        self.twist.linear.x = x_velocity
        self.twist.linear.y = y_velocity
        self.twist.linear.z = self.kp * (5.0 - self.pz)
        self.twist.angular.z = 0.0
        # if self.twist.linear.x > self.vmax:
        #     self.twist.linear.x = self.vmax
        # elif self.twist.linear.x < -self.vmax:
        #     self.twist.linear.x = -self.vmax
        # if self.twist.linear.y > self.vmax:
        #     self.twist.linear.y = self.vmax
        # elif self.twist.linear.y < -self.vmax:
        #     self.twist.linear.y = -self.vmax
       
    def get_control_vel(self, actorx, actory, actorz):
        uav_dis_curtar = math.sqrt((actorx - self.uav_current_pose.x)**2 + (actory - self.uav_current_pose.y)**2)  #distance
        #temp = self.VectDiff(self.target_position.linear, self.uav_current_pose)  # vector
        uav_vel_total = self.kp * uav_dis_curtar  # velocity
        if uav_vel_total > self.vmax:
            uav_vel_total = self.vmax       
        turnover_flag = False
        target_yaw = self.pos2ang(actorx, actory, self.uav_current_pose.x, self.uav_current_pose.y)
        mid_yaw = target_yaw - self.uav_current_yaw
        if mid_yaw > math.pi:
            mid_yaw = mid_yaw - 2*math.pi
        elif mid_yaw < -math.pi:
            mid_yaw = 2*math.pi + mid_yaw
        self.twist.angular.z = self.kpy * mid_yaw
        if self.twist.angular.z > self.anglemax:
            self.twist.angular.z = self.anglemax
        elif self.twist.angular.z < -self.anglemax:
            self.twist.angular.z = -self.anglemax
        self.twist.linear.x  = uav_vel_total * math.cos(mid_yaw)
        self.twist.linear.y = uav_vel_total * math.sin(mid_yaw)
        self.twist.linear.z = self.kp * (actorz - self.uav_current_pose.z)


    def pos2ang(self, xa, ya, xb, yb):   #([xb,yb] to [xa, ya])
        if not xa-xb == 0:
            angle = math.atan2((ya - yb),(xa - xb))
            if (ya-yb > 0) and (angle < 0):
                angle = angle + math.pi
            elif (ya-yb < 0) and (angle > 0):
                angle = angle - math.pi
            elif ya-yb == 0:
                if xa-xb > 0:
                    angle = 0.0
                else:
                    angle = math.pi
        else:
            if ya-yb > 0:
                angle = math.pi / 2
            elif ya-ya <0:
                angle = -math.pi / 2
            else:
                angle = 0.0
        if angle < 0:
            angle = angle + 2 * math.pi   # 0 to 2pi
        return angle

    def is_inObsatcle(self, x, y, safe_dis=1.5):#判断飞机是否在障碍物内4.11
        for obstacle in self.black_box:
            if (obstacle[0][0]-safe_dis <= x <= obstacle[0][1+safe_dis]) and (obstacle[1][0]-safe_dis <= y <= obstacle[1][1]+safe_dis):
                return True
        return False
    
if __name__ == "__main__": 
    run = Track_and_Annouce(sys.argv[1], sys.argv[2])
    run.loop()
   
        
        
